`timescale 1 ns / 1 ps
module cbc_dig_tb();

////////////////////////////////////////////////
// Define any interconnects wider than 1-bit //
//////////////////////////////////////////////
wire [1:0] eep_addr;
wire [13:0] eep_rd_data;
wire [13:0] dst;
wire [15:0] rsp;
wire [13:0] duty;

/////////////////////////////////////////////
// Define any registers used in testbench //
///////////////////////////////////////////
reg [23:0] cmd_data;		// used to provide commands/data to cfg_UART of DUT
reg snd_frm;			// kicks off command/data transmission to DUT 
reg clk,rst_n;
reg [7:0] accel_amnt;
reg accel_snd;

//////////////////////
// Instantiate DUT //
////////////////////
cbc_dig DUT(.clk(clk), .rst_n(rst_n), .RX_A(RX_A), .RX_C(RX_C), .TX_C(TX_C),
	.CH_A(CH_A), .CH_B(CH_B), .dst(dst), .eep_rd_data(eep_rd_data),
	.eep_addr(eep_addr), .eep_cs_n(eep_cs_n), .eep_r_w_n(eep_r_w_n),
	.chrg_pmp_en(chrg_pmp_en));
        
///////////////////////////////
// Instantiate EEPROM Model //
/////////////////////////////
eep iEEP(.clk(clk), .por_n(rst_n), .eep_addr(eep_addr), .wrt_data(dst),  
			.rd_data(eep_rd_data), .eep_cs_n(eep_cs_n),
			.eep_r_w_n(eep_r_w_n), .chrg_pmp_en(chrg_pmp_en));

////////////////////////////////
// Instantiate Config Master //
//////////////////////////////
cfg_mstr iCFG(.clk(clk), .rst_n(rst_n), .cmd_data(cmd_data), .snd_frm(snd_frm),
	      .RX_C(TX_C), .TX_C(RX_C), .rsp(rsp), .rsp_rdy(rsp_rdy));

///////////////////////////////
// Instantiate Accel Master //
/////////////////////////////
accel_mstr iACCEL(.clk(clk), .rst_n(rst_n), .TX_A(RX_A), .snd(accel_snd), .amnt(accel_amnt));

//////////////////////////////
// Instantiate PWM monitor //
////////////////////////////
pwm_monitor iMON(.clk(clk), .rst_n(rst_n), .CH_A(CH_A), .CH_B(CH_B),
	.duty(duty), .duty_valid(duty_valid));


always
  ///////////////////
  // 500MHz clock // 
  /////////////////
  #1 clk = ~clk;
  initial clk = 0;


/////////////////////////////////////////////////////////////////
// The following section actually implements the real testing //
///////////////////////////////////////////////////////////////
initial
  begin
	
	normalOP();
	setXOP();
	
	cmdModeOP();
	@(posedge clk)
	$finish;
	
    ///////////////////////////
    // Do your testing here //
    /////////////////////////
	end
	

	task _initTst;
		begin
			rst_n = 0;
			@(posedge clk)
            @(posedge clk)
            rst_n = 1;
			@(posedge duty_valid)
			rst_n = 1;
		end
	endtask

	task normalOP;
		integer cnt;
		reg [13:0] dutyOut[0:100];
				
		begin
			_initTst();
			$display("starting normal Operation");
			//$monitor("PWM mag: %h", DUT.iPWM.magnitude);
			accel_amnt = 100;
			accel_snd = 1;
			@(posedge clk)
			accel_snd = 0;
			for( cnt = 0; cnt < 25; cnt = cnt + 1) begin
		        @(posedge duty_valid)
		        dutyOut[cnt] = duty;
		        $display("Duty = %h",duty);
		    end
		    $writememh("testOutput.data",dutyOut);
		    $display("finished normal Operation");
		end
	endtask

	task setXOP;
		integer cnt;
		reg [23:0] commands [0:25];
		begin
			_initTst();
			$readmemh("setXcommands.data", commands);
			$display("starting setXOP");
			snd_frm = 0;
			for(cnt = 0; cnt < 4; cnt = cnt + 1) begin
				accel_amnt = 100;
				accel_snd = 1;
				@(posedge clk)
				accel_snd = 0;
				repeat(5) begin
					@(posedge duty_valid)
					$display("Duty = %h", duty);
				end
				cmd_data = commands[cnt];
				snd_frm = 1;
				@(posedge clk)
				@(posedge clk)
				snd_frm = 0; // Send frame 1 accel vals early
				//repeat(1) begin
				//	@(posedge duty_valid)
				//	$display("Duty = %h", duty);
				//end
				cmd_data = commands[cnt];
                if(cmd_data[19:16] == 4'hC)
                    $display("setX to %h", cmd_data[13:0]);
                else
                    $display("non setX command");
				@(posedge rsp_rdy)
				$display("responded: %h", rsp);
			end
		end
	endtask

	task cmdModeOP;
		integer cnt;
		reg [23:0] commands [0:50];
		begin
			_initTst();
			$readmemh("commands.data", commands);
			$display("starting cmdModeOP");
			accel_snd = 1;
			accel_amnt = 5;
			@(posedge clk) accel_snd = 0;
			for(cnt = 0; cnt < 20; cnt = cnt + 1) begin
				snd_frm = 1;
				cmd_data = commands[cnt];
				$display("Sent command %h", cmd_data);
				@(posedge clk)
				@(posedge clk)
				snd_frm = 0;
				@(posedge rsp_rdy)
				$display("Responded: %h", rsp);
			end

		end	
	endtask	


endmodule
